from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription,TimerAction
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import DeclareLaunchArgument,ExecuteProcess
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory
import os
import yaml

if os.path.exists('config.yaml'):
    config_file = 'config.yaml'
    with open(config_file, 'r', encoding='utf-8') as file:
        config = yaml.safe_load(file)
    
    # 如果 config.yaml 中没有 px4_paths，则读取 config_template.yaml
    if "px4_paths" not in config or not config["px4_paths"]:
        if os.path.exists('config_template.yaml'):
            with open('config_template.yaml', 'r', encoding='utf-8') as file:
                config = yaml.safe_load(file)
        else:
            raise KeyError("config.yaml 中没有 px4_paths 且找不到 config_template.yaml")

# 如果 config.yaml 不存在，则读取 config_template.yaml
elif os.path.exists('config_template.yaml'):
    config_file = 'config_template.yaml'
    with open(config_file, 'r', encoding='utf-8') as file:
        config = yaml.safe_load(file)
else:
    raise FileNotFoundError("找不到配置文件：config.yaml 或 config_template.yaml")

config["px4_paths"]


    #编写launch文件 运行  ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --qx 0 --qy 0 --qz 0 --qw 1 --frame-id world --child-frame-id mystaticturtle

def generate_launch_description():
    ugv_ids = ['ugv1']
    px4_path = LaunchConfiguration('px4_path')
    px4_path_launch_arg = DeclareLaunchArgument(
        'px4_path',
        default_value= config["px4_paths"],
        description='PX4 Firmware Path'
    )

    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])

    # delayed_sub_launch = TimerAction(
    #     period=5.0,  # 延迟时间（秒）
    #     actions=[
    #         # 包含要延迟启动的子launch文件
    #         IncludeLaunchDescription(
    #             PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
    #             launch_arguments={'uav_id': 'uav1', 'uav_pose': '0,0,1', 'uav_num': '0', 'px4_path': px4_path, 'sim_model':'x500_RGBD','sim_world':'industrialwarehouse'}.items()
    #         )
    #     ]
    # )
    
    return LaunchDescription([
        px4_path_launch_arg,

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={'world_sdf': 'industrialwarehouse.sdf','bridge_config':'1ugv2uav_rgbd.yaml'}.items()
        ),
        #delayed_sub_launch,

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
            launch_arguments={'uav_id': 'uav1', 'uav_pose': '2,0,1,0,0,0', 'uav_num': '0', 'px4_path': px4_path, 'sim_model':'x500_RGBD','sim_world':'industrialwarehouse'}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
            launch_arguments={'uav_id': 'uav2', 'uav_pose': '-3,0,1,0,0,3.14', 'uav_num': '1', 'px4_path': px4_path, 'sim_model':'x500_RGBD','sim_world':'industrialwarehouse'}.items()
        ),
        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv1',
                'ugv_pose': '-1.5,2.0,0,0,0,0',
                'model_name': 'ugv_RGBD'
            }.items()
        ),
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'ugv_allstart.launch.py']),
            launch_arguments={
                'ugv_ids': str(ugv_ids)  # ROS 2 支持 list 作为参数值（YAML 格式）
            }.items()
        ),
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'addi_ugv12_bridge.launch.py'])
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', 
                 '--frame-id', 'x500_RGBD_0/camera_link', '--child-frame-id', 'x500_RGBD_0/camera_link/realsense_d435', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0.5', '--qy', '-0.5', '--qz', '0.5', '--qw', '0.5', 
                 '--frame-id', 'x500_RGBD_0/camera_link', '--child-frame-id', 'x500_RGBD_0/camera_link/odom', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),
        # ExecuteProcess(
        #     cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'world', '--child-frame-id', 'x500_RGBD_0/odom', '--ros-args', '--remap', 'use_sim_time:=true'],
        # ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', 
                 '--frame-id', 'x500_RGBD_0/base_footprint', '--child-frame-id', 'x500_RGBD_0', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),
        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', 
                 '--frame-id', 'x500_RGBD_0', '--child-frame-id', 'x500_RGBD_0/camera_link', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),

        
#######################
        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', 
                 '--frame-id', 'x500_RGBD_1/camera_link', '--child-frame-id', 'x500_RGBD_1/camera_link/realsense_d435', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0.5', '--qy', '-0.5', '--qz', '0.5', '--qw', '0.5', 
                 '--frame-id', 'x500_RGBD_1/camera_link', '--child-frame-id', 'x500_RGBD_1/camera_link/odom', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),
        # ExecuteProcess(
        #     cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'world', '--child-frame-id', 'x500_RGBD_1/odom', '--ros-args', '--remap', 'use_sim_time:=true'],
        # ),
        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
                   '--frame-id', 'x500_RGBD_1', '--child-frame-id', 'x500_RGBD_1/camera_link', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
                   '--frame-id', 'x500_RGBD_1/base_footprint', '--child-frame-id', 'x500_RGBD_1', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),
        # ExecuteProcess(
        #     cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
        #          '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', 
        #          '--frame-id', 'odom', '--child-frame-id', 'x500_RGBD_1/base_footprint', '--ros-args', '--remap', 'use_sim_time:=true'],
        # ),

#################################
        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', 
                 '--frame-id', 'ugv1/camera_link', '--child-frame-id', 'ugv1/camera_link/realsense_d435', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0.5', '--qy', '-0.5', '--qz', '0.5', '--qw', '0.5', 
                 '--frame-id', 'ugv1/camera_link', '--child-frame-id', 'ugv1/camera_link/odom', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),
        # ExecuteProcess(
        #     cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'world', '--child-frame-id', 'x500_RGBD_1/odom', '--ros-args', '--remap', 'use_sim_time:=true'],
        # ),
        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
                   '--frame-id', 'ugv1', '--child-frame-id', 'ugv1/camera_link', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', 
                 '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
                   '--frame-id', 'ugv1/base_footprint', '--child-frame-id', 'ugv1', '--ros-args', '--remap', 'use_sim_time:=true'],
        ),



        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'microagent.launch.py'])
        ),

        # IncludeLaunchDescription(
        #     PathJoinSubstitution([launch_dir, 'uav.launch.py']),
        #     launch_arguments={'vehicleID': 'uav1', 'local_host_mode': 'True', 'multi_simulation_mode': 'True'}.items()
        # ),


    ])